/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package botlab.command;

import com.jme3.bullet.control.RigidBodyControl;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Spatial;
import java.util.Random;

/**
 *
 * @author sebastien
 */
public class SeekCommand extends BaseCommand {

    private float maxSpeed;
    private Vector3f target;
    private Vector3f position;
    private Vector3f velocity;
    private Vector3f direction;

    public SeekCommand(Vector3f target, float maxSpeed) {
        this.target = target;
        this.maxSpeed = maxSpeed;

        velocity = new Vector3f();
        direction = new Vector3f();
    }

    public void setPosition(Vector3f position) {
        this.position = position;
    }

    public void setDirection(Vector3f direction) {
        this.direction = direction;
    }

    public void setVelocity(Vector3f velocity) {
        this.velocity = velocity;
    }

    public void setTarget(Vector3f target) {
        this.target = target;
    }

    public Vector3f seek() {
        Vector3f desiredVelocity = target.subtract(position);

        return this.applyVelocity(desiredVelocity);
    }

    public Vector3f applyVelocity(Vector3f desiredVelocity) {

        Vector3f steering = new Vector3f(desiredVelocity);
        steering = steering.normalize();
        steering = steering.mult(maxSpeed);

        this.setVelocity(steering);

        return steering;
    }

    @Override
    public void doCommand(Spatial spatial, float tpf) {
        if (spatial.getControl(RigidBodyControl.class) == null) {
            throw new IllegalStateException("Not supported");
        } else {
            /*
             Vector3f position = spatial.getControl(RigidBodyControl.class).getPhysicsLocation();
             Vector3f direction = seek(position);
             if (direction.length()>=3)
             {
             spatial.getControl(RigidBodyControl.class).setLinearVelocity(direction);
             }
             else
             spatial.getControl(RigidBodyControl.class).setLinearVelocity(Vector3f.ZERO);
             Quaternion quat = new Quaternion();
             quat.lookAt(direction, new Vector3f(0,0,1));
             spatial.getControl(RigidBodyControl.class).setPhysicsRotation(quat);*/

            setPosition(spatial.getLocalTranslation());
            //setTarget(wander());
            setDirection(seek());

            setPosition(spatial.getLocalTranslation());
            spatial.setLocalTranslation(spatial.getLocalTranslation().add(direction.mult(tpf)));

            Quaternion quat = new Quaternion();
            quat.lookAt(direction, new Vector3f(0, 1, 0));
            spatial.setLocalRotation(quat);
        }

    }
}
